
#include "ctrl_bsp_joys.h"
#include "oslib.h"

#include "adc.h"

#include <string.h>
#include <math.h>

#define ADC_JOYSTICK_SAMPLE_NUMBER 5

extern uint8_t calib_type;

/* 摇杆ADC采样缓冲区, ADC使用DMA方式, 直接采集到这个缓冲区 */
static ADC_Joystick_Value_t adc_joystick_sample[ADC_JOYSTICK_SAMPLE_NUMBER] = {0};
/* 摇杆ADC采样值 */
static ADC_Joystick_Value_t adc_joystick_value;
/* 摇杆ADC采集偏移值, 从Flash中读取 */
static ADC_Joystick_Value_t adc_joystick_err = {0, 0, 0, 0};

/**
 * @brief 将摇杆ADC采样值限制在0x000-0xFFF内
 * 
 * @param value     采样值
 * @return int16_t  限制范围后的采样值
 */
static int16_t Joystick_Limit(int16_t value)
{
    return ((value < 0) ? 0 : ((value > 0xFFF) ? 0xFFF : value));
}

/* 摇杆ADC偏移值在Flash中存储的起始位置 */
#define FLASH_Param ((uint16_t *)(0x08010000))
/**
 * @brief 将当前的ADC偏移值写入Flash
 * @return int 0-成功,-1-失败
 */
static int Joystick_WriteParam()
{
    FLASH_EraseInitTypeDef EraseInitStruct;
    uint32_t PageError;

    // osKernelLock();     // 调度锁上锁
    HAL_FLASH_Unlock(); // FLASH解锁

    EraseInitStruct.TypeErase = FLASH_TYPEERASE_PAGES;
    EraseInitStruct.PageAddress = (uint32_t)(FLASH_Param);
    EraseInitStruct.NbPages = 1;
    if (HAL_OK != HAL_FLASHEx_Erase(&EraseInitStruct, &PageError))
    {
        HAL_FLASH_Lock(); // FLASH上锁
        // osKernelUnlock(); // 调度锁解锁
        return -1;
    }
    HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, (uint32_t)&FLASH_Param[0], (int16_t)adc_joystick_err.adc_joy_right_fb);
    HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, (uint32_t)&FLASH_Param[1], (int16_t)adc_joystick_err.adc_joy_right_lr);
    HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, (uint32_t)&FLASH_Param[2], (int16_t)adc_joystick_err.adc_joy_left_fb);
    HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, (uint32_t)&FLASH_Param[3], (int16_t)adc_joystick_err.adc_joy_left_lr);
    HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, (uint32_t)&FLASH_Param[4], (uint16_t)calib_type);

    HAL_FLASH_Lock(); // FLASH上锁
    // osKernelUnlock(); // 调度锁解锁
    return 0;
}

/**
 * @brief 从Flash中读取摇杆ADC偏移值
 */
static void Joystck_ReadParam(void)
{
    adc_joystick_err.adc_joy_right_fb = (int16_t)FLASH_Param[0];
    adc_joystick_err.adc_joy_right_lr = (int16_t)FLASH_Param[1];
    adc_joystick_err.adc_joy_left_fb = (int16_t)FLASH_Param[2];
    adc_joystick_err.adc_joy_left_lr = (int16_t)FLASH_Param[3];
    calib_type = (uint8_t)FLASH_Param[4];
}

/**
 * @brief 对摇杆ADC采样值进行平均处理和偏差校正
 * @return ADC_Joystick_Value_t 处理后的摇杆ADC值, 可以直接用于控制报文
 */
ADC_Joystick_Value_t Joystick_Sample(void)
{
    ADC_Joystick_Value_t adc_sum = {0, 0, 0, 0};
    for (int i = 0; i < ADC_JOYSTICK_SAMPLE_NUMBER; i++)
    {
        adc_sum.adc_joy_left_fb += adc_joystick_sample[i].adc_joy_left_fb;
        adc_sum.adc_joy_left_lr += adc_joystick_sample[i].adc_joy_left_lr;
        adc_sum.adc_joy_right_fb += adc_joystick_sample[i].adc_joy_right_fb;
        adc_sum.adc_joy_right_lr += adc_joystick_sample[i].adc_joy_right_lr;
    }
    adc_joystick_value.adc_joy_left_fb = adc_sum.adc_joy_left_fb / ADC_JOYSTICK_SAMPLE_NUMBER;
    adc_joystick_value.adc_joy_left_lr = adc_sum.adc_joy_left_lr / ADC_JOYSTICK_SAMPLE_NUMBER;
    adc_joystick_value.adc_joy_right_fb = adc_sum.adc_joy_right_fb / ADC_JOYSTICK_SAMPLE_NUMBER;
    adc_joystick_value.adc_joy_right_lr = adc_sum.adc_joy_right_lr / ADC_JOYSTICK_SAMPLE_NUMBER;

    adc_joystick_value.adc_joy_left_fb = Joystick_Limit(adc_joystick_value.adc_joy_left_fb + adc_joystick_err.adc_joy_left_fb);
    adc_joystick_value.adc_joy_left_lr = Joystick_Limit(adc_joystick_value.adc_joy_left_lr + adc_joystick_err.adc_joy_left_lr);
    adc_joystick_value.adc_joy_right_fb = Joystick_Limit(adc_joystick_value.adc_joy_right_fb + adc_joystick_err.adc_joy_right_fb);
    adc_joystick_value.adc_joy_right_lr = Joystick_Limit(adc_joystick_value.adc_joy_right_lr + adc_joystick_err.adc_joy_right_lr);

    return adc_joystick_value;
}

/**
 * @brief 对应摇杆ADC采样值进行平均处理, 但不校正偏差
 * @return ADC_Joystick_Value_t 处理后的摇杆ADC值, 可以用于自动校准
 */
ADC_Joystick_Value_t Joystick_Raw_Sample(void)
{
    ADC_Joystick_Value_t adc_sum = {0, 0, 0, 0};
    for (int i = 0; i < ADC_JOYSTICK_SAMPLE_NUMBER; i++)
    {
        adc_sum.adc_joy_left_fb += adc_joystick_sample[i].adc_joy_left_fb;
        adc_sum.adc_joy_left_lr += adc_joystick_sample[i].adc_joy_left_lr;
        adc_sum.adc_joy_right_fb += adc_joystick_sample[i].adc_joy_right_fb;
        adc_sum.adc_joy_right_lr += adc_joystick_sample[i].adc_joy_right_lr;
    }
    adc_joystick_value.adc_joy_left_fb = adc_sum.adc_joy_left_fb / ADC_JOYSTICK_SAMPLE_NUMBER;
    adc_joystick_value.adc_joy_left_lr = adc_sum.adc_joy_left_lr / ADC_JOYSTICK_SAMPLE_NUMBER;
    adc_joystick_value.adc_joy_right_fb = adc_sum.adc_joy_right_fb / ADC_JOYSTICK_SAMPLE_NUMBER;
    adc_joystick_value.adc_joy_right_lr = adc_sum.adc_joy_right_lr / ADC_JOYSTICK_SAMPLE_NUMBER;

    return adc_joystick_value;
}

int Joystick_Calibration(ADC_Joystick_Value_t err)
{
    adc_joystick_err = err;
    return Joystick_WriteParam();
}

/**
 * @brief 摇杆初始化
 */
void Joystick_Init(void)
{
    Joystck_ReadParam();
    uprintf("Now err %hd %hd %hd %hd\r\n", adc_joystick_err.adc_joy_right_fb, adc_joystick_err.adc_joy_right_lr, adc_joystick_err.adc_joy_left_fb, adc_joystick_err.adc_joy_left_lr);
    uprintf("Type: %hhd\r\n", calib_type);
    HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc_joystick_sample, sizeof(adc_joystick_sample) / sizeof(uint16_t));
}